#ifndef HB_MOUNT_SIYI_H
#define HB_MOUNT_SIYI_H
#include <stdio.h>
#include <float.h>
#include "camera_shareddefs.h"
#include <stdlib.h>
#include <sys/file.h>
#include <sys/types.h>
#include <sys/statfs.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <pthread.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <signal.h>
#include <termios.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <assert.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <termios.h>
#include "circular_buf.h"

#include <opencv2/opencv.hpp>



#define MOUNT_SIYI_PACKETLEN_MAX     38  // maximum number of bytes in a packet sent to or received from the gimbal
class MountSiyi
{
public:
	MountSiyi();
	~MountSiyi();

	// init - performs any required initialisation for this instance
    bool init(const char *sockType, const char *ip);

	//subscription data from gimbal.
	void update();

    // return true if healthy
    bool healthy();

	typedef void (*siyiVideoCallback)(cv::Mat image);

	bool registerSiyiVideoCallback(siyiVideoCallback function);

	typedef void(*siyiAttitudeCallback)(float roll, float pitch, float yaw);

	bool registerSiyiAttitudeCallback(siyiAttitudeCallback function);
	//
    // camera controls
    //

    // take a picture.  returns true on success
    bool take_picture();

	// start or stop video recording
    // set start_recording = true to start record, false to stop recording
    bool record_video(bool start_recording);

	// set zoom specified as a rate or percentage
    bool set_zoom(ZoomType zoom_type, float zoom_value);

	// set focus specified as rate, percentage or auto
    // focus in = -1, focus hold = 0, focus out = 1
    SetFocusResult set_focus(FocusType focus_type, float focus_value);

	// set camera lens as a value from 0 to 8.  ZT30 only
    bool set_lens(uint8_t lens);

	// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
    // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
    bool set_camera_source(uint8_t primary_source, uint8_t secondary_source);

    //
    // rangefinder
    //

    // get rangefinder distance.  Returns true on success
    bool get_rangefinder_distance(float& distance_m);

	bool startRtspRecv(const char *rtsp_url);

	bool getRtspHealthy();
private:
	// serial protocol command ids
    enum class SiyiCommandId {
    	HEARTBEAT = 0x00,
        ACQUIRE_FIRMWARE_VERSION = 0x01,
        HARDWARE_ID = 0x02,
        AUTO_FOCUS = 0x04,
        MANUAL_ZOOM_AND_AUTO_FOCUS = 0x05,
        MANUAL_FOCUS = 0x06,
        GIMBAL_ROTATION = 0x07,
        CENTER = 0x08,
        ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A,
        FUNCTION_FEEDBACK_INFO = 0x0B,
        PHOTO = 0x0C,
        ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
        ABSOLUTE_ZOOM = 0x0F,
        SET_CAMERA_IMAGE_TYPE = 0x11,
        READ_RANGEFINDER = 0x15,
        EXTERNAL_ATTITUDE = 0x22,
        SET_TIME = 0x30,
    };

    // Function Feedback Info packet info_type values
    enum class FunctionFeedbackInfo : uint8_t {
        SUCCESS = 0,
        FAILED_TO_TAKE_PHOTO = 1,
        HDR_ON = 2,
        HDR_OFF = 3,
        FAILED_TO_RECORD_VIDEO = 4
    };

	// Photo Function packet func_type values
    enum class PhotoFunction : uint8_t {
        TAKE_PICTURE = 0,
        HDR_TOGGLE = 1,
        RECORD_VIDEO_TOGGLE = 2,
        LOCK_MODE = 3,
        FOLLOW_MODE = 4,
        FPV_MODE = 5
    };

    // parsing state
    enum class ParseState : uint8_t {
        WAITING_FOR_HEADER_LOW,
        WAITING_FOR_HEADER_HIGH,
        WAITING_FOR_CTRL,
        WAITING_FOR_DATALEN_LOW,
        WAITING_FOR_DATALEN_HIGH,
        WAITING_FOR_SEQ_LOW,
        WAITING_FOR_SEQ_HIGH,
        WAITING_FOR_CMDID,
        WAITING_FOR_DATA,
        WAITING_FOR_CRC_LOW,
        WAITING_FOR_CRC_HIGH,
    };

	// hardware model enum
    enum class HardwareModel : uint8_t {
        UNKNOWN = 0,
        A2,
        A8,
        ZR10,
        ZR30,
        ZT6,
        ZT30
    } _hardware_model;

    enum class HdrStatus : uint8_t {
        OFF = 0,
        ON  = 1,
    };

    enum class RecordingStatus : uint8_t {
        OFF       = 0,
        ON        = 1,
        NO_CARD   = 2,
        DATA_LOSS = 3,
    };

    enum class GimbalMotionMode : uint8_t {
        LOCK   = 0,
        FOLLOW = 1,
        FPV    = 2,
    };

    enum class GimbalMountingDirection : uint8_t {
        UNDEFINED = 0,
        NORMAL = 1,
        UPSIDE_DOWN = 2,
    };

    enum class VideoOutputStatus : uint8_t {
        HDMI = 0,
        CVBS = 1,
    };

	// Response message for "Acquire Gimbal Confuguration Information" (0x0A)
    typedef struct {
        uint8_t _reserved1;
        HdrStatus hdr_status;
        uint8_t _reserved3;
        RecordingStatus record_status;
        GimbalMotionMode motion_mode;
        GimbalMountingDirection mounting_dir;
        VideoOutputStatus video_mode;
    } GimbalConfigInfo;
    static_assert(sizeof(GimbalConfigInfo) == 7, "GimbalConfigInfo must be 7 bytes");

    // camera image types (aka lens)
    enum class CameraImageType : uint8_t {
        MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE = 0,
        MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM = 1,
        MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL = 2,
        MAIN_ZOOM_SUB_THERMAL = 3,
        MAIN_ZOOM_SUB_WIDEANGLE = 4,
        MAIN_WIDEANGLE_SUB_THERMAL = 5,
        MAIN_WIDEANGLE_SUB_ZOOM = 6,
        MAIN_THERMAL_SUB_ZOOM = 7,
        MAIN_THERMAL_SUB_WIDEANGLE = 8
    };

    typedef struct {
        uint8_t major;
        uint8_t minor;
        uint8_t patch;
    } Version;
    typedef struct {
        Version camera;
        Version gimbal;
        Version zoom;
        bool received; // true once version information has been received
    } FirmwareVersion;

	typedef struct Vector3f {
    float x;
    float y;
    float z;
	} Vector3f;

	typedef enum InterfaceType{
		UNKNOW = 0,
		SIYI_TCP = 1,
		SIYI_UDP = 2
	}InterfaceType;
public:
	// reading incoming packets from gimbal and confirm they are of the correct format
    // results are held in the _parsed_msg structure
    void read_incoming_packets(uint8_t *buf, uint16_t nbytes);

    // process successfully decoded packets held in the _parsed_msg structure
    void process_packet();

    // send packet to gimbal
    // returns true on success, false if outgoing serial buffer is full
    bool send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len);
    bool send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte);

    // request info from gimbal
    void request_firmware_version() { send_packet(SiyiCommandId::ACQUIRE_FIRMWARE_VERSION, nullptr, 0); }
    void request_hardware_id() { send_packet(SiyiCommandId::HARDWARE_ID, nullptr, 0); }
    void request_configuration() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO, nullptr, 0); }
    void request_function_feedback_info() { send_packet(SiyiCommandId::FUNCTION_FEEDBACK_INFO, nullptr, 0); }
    void request_gimbal_attitude() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE, nullptr, 0); }
    void request_rangefinder_distance() { send_packet(SiyiCommandId::READ_RANGEFINDER, nullptr, 0); }
	void send_heartBeat(){ send_packet(SiyiCommandId::HEARTBEAT, nullptr, 0); }

	// rotate gimbal.  pitch_rate and yaw_rate are scalars in the range -100 ~ +100
    // yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
    void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef);

	// center gimbal
    void center_gimbal();

    // Set gimbal's motion mode if it has changed. Use force=true to always send.
    //   FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
    //   LOCK: roll, pitch and yaw are all in earth-frame
    //   FPV: roll, pitch and yaw are all in body-frame
    // Returns true if message successfully sent to Gimbal
    bool set_motion_mode(const GimbalMotionMode mode, const bool force=false);

    // send target pitch and yaw rates to gimbal
    // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame
    void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef);

    // send target pitch and yaw angles to gimbal
    // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame
    void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef, float flight_yaw);

    // send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1
    bool send_zoom_rate(float zoom_value);

    // send zoom multiple command to camera. e.g. 1x, 10x, 30x
    bool send_zoom_mult(float zoom_mult);

    // get zoom multiple max
    float get_zoom_mult_max();

    // update zoom controller
    void update_zoom_control();

    // get model name string, returns nullptr if hardware id is unknown
    const char* get_model_name();

    // Checks that the firmware version on the Gimbal meets the minimum supported version.
    void check_firmware_version();

	void handlerRecv();
	void handlerSend();

	bool tcpClient(const char *ip, const char *port);

	bool udpClient(const char *ip, const char *port);


	bool set_motion_mode_lock();

	bool set_motion_mode_follow();

	bool set_motion_mode_fpv();


private:
	bool _initialised;                              // true once the driver has been initialised
    bool _got_hardware_id;                          // true once hardware id ha been received

	// absolute zoom control.  only used for A8 that does not support abs zoom control
    ZoomType _zoom_type;                            // current zoom type
    float _zoom_rate_target;                        // current zoom rate target
    float _zoom_mult;                               // most recent actual zoom multiple received from camera
    uint32_t _last_zoom_control_ms;                 // system time that zoom control was last run


	// rangefinder variables
    uint32_t _last_rangefinder_req_ms;              // system time of last request for rangefinder distance
    uint32_t _last_rangefinder_dist_ms;             // system time of last successful read of rangefinder distance
    float _rangefinder_dist_m;                      // distance received from rangefinder

	// buffer holding bytes from latest packet.  This is only used to calculate the crc
    uint8_t _msg_buff[MOUNT_SIYI_PACKETLEN_MAX];
    uint8_t _msg_buff_len;
    const uint8_t _msg_buff_data_start = 8;         // data starts at this byte of _msg_buff

    // parser state and unpacked fields
    struct PACKED {
        uint16_t data_len;                          // expected number of data bytes
        uint8_t command_id;                         // command id
        uint16_t data_bytes_received;               // number of data bytes received so far
        uint16_t crc16;                             // latest message's crc
        ParseState state;                           // state of incoming message processing
    } _parsed_msg;

	// variables for sending packets to gimbal
    uint32_t _last_send_ms;                         // system time (in milliseconds) of last packet sent to gimbal
    uint16_t _last_seq;                             // last sequence number used (should be increment for each send)

	// actual attitude received from gimbal
    Vector3f _current_angle_rad;                    // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
    Vector3f _current_rates_rads;                   // current angular rates in rad/s (x=roll, y=pitch, z=yaw)
    uint32_t _last_current_angle_rad_ms;            // system time _current_angle_rad was updated
    uint32_t _last_req_current_angle_rad_ms;        // system time that this driver last requested current angle


	// Configuration info received from gimbal
    GimbalConfigInfo _config_info;

	FirmwareVersion _fw_version; 

	// hardware lookup table indexed by HardwareModel enum values (see above)
    struct HWInfo {
        uint8_t hwid[2];
        const char* model_name;
    };
    static const HWInfo hardware_lookup_table[];

    // count of SET_TIME packets, we send 5 times to cope with packet loss
    uint8_t sent_time_count;


	InterfaceType m_interfaceType;

	//tcp udp
	int m_tcpSockfd;
	int m_udpSockfd;
	struct sockaddr_in m_saddr;
	pthread_t m_RecvRunTid;
	pthread_t m_SendRunTid;

	struct circularBuffer m_sendBuffer;

	//callback function
	siyiVideoCallback m_siyiVideoCallbackFunction;
	siyiAttitudeCallback m_siyiAttitudeCallbackFunction;

	bool m_rtspHealthy;
};


extern MountSiyi mountSiyiHandler;

#endif //HB_MOUNT_SIYI_H
